﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using WheelOfCivilization.Map;
using Microsoft.Xna.Framework;
using WheelOfCivilization.Map.MapObject.Movable;
using System.Threading;

namespace WheelOfCivilization.Logic
{
    static class MovementLogic
    {
        #region preprocessing
        //<summary>
        //counting transition points for clusters, this should be probabbly caled from control with list of newly created clusters, 
        //but I'm not sure now if it works correctly or as I intended
        //</summary>
        public static void clusterGates(List<WorldCluster> list)
        {
            foreach (WorldCluster wc in list)
            {
                if (wc.neigbourhood[0] != null)
                {
                    for (int i = 0; i < 10; i++)
                    {
                        if (!wc.unmovableContentGS[i, 0].occupied && !wc.neigbourhood[0].unmovableContentGS[i, 9].occupied)
                        {
                            wc.bordersGS[0].Add(new int[] {i, 0});
                            wc.neigbourhood[0].bordersGS[2].Add(new int[] { i, 9 });
                        }
                    }
                }

                if (wc.neigbourhood[2] != null)
                {
                    for (int i = 0; i < 10; i++)
                    {
                        if (!wc.unmovableContentGS[9, i].occupied && !wc.neigbourhood[2].unmovableContentGS[0, i].occupied)
                        {
                            wc.bordersGS[1].Add(new int[] { 9, i });
                            wc.neigbourhood[2].bordersGS[3].Add(new int[] { 0, i });
                        }
                    }
                }
            }
        }
        #endregion

        #region logic questions
        //<summary>
        //if I want to go through conrner (eg from 0,0 to 1,1) of the cluster, I need to know, if I can (almost the same as borders)
        //</summary>
        private static bool isCornerEmpty(WorldCluster worldCluster1, WorldCluster worldCluster2)
        {
            byte orientation = 255;
            for (int i = 0; i < 4; i++)
            {
                if (worldCluster1.neigbourhood[i * 2 + 1] == worldCluster2)
                {
                    orientation = (byte)(i * 2 + 1);
                    break;
                }
            }
            if (orientation == 255)
                return false;
            else
            {
                switch (orientation)
                {
                    case 1:
                        {
                            if (worldCluster1.unmovableContentGS[9, 0].occupied)
                                return false;
                            if (worldCluster2.unmovableContentGS[0, 9].occupied)
                                return false;
                            break;
                        }

                    case 3:
                        {
                            if (worldCluster1.unmovableContentGS[9, 9].occupied)
                                return false;
                            if (worldCluster2.unmovableContentGS[0, 0].occupied)
                                return false;
                            break;
                        }

                    case 5:
                        {
                            if (worldCluster1.unmovableContentGS[0, 9].occupied)
                                return false;
                            if (worldCluster2.unmovableContentGS[9, 0].occupied)
                                return false;
                            break;
                        }

                    case 7:
                        {
                            if (worldCluster1.unmovableContentGS[0, 0].occupied)
                                return false;
                            if (worldCluster2.unmovableContentGS[9, 9].occupied)
                                return false;
                            break;
                        }
                }
            }
            return true;
        }

        //<summary>
        //if I need to know if there is border crossing
        //</summary>
        private static bool haveCoomonBorder(WorldCluster source, WorldCluster tmp)
        {
            for (int i = 0; i < 7; i = i + 2)
            {
                if (source.neigbourhood[i] == tmp)
                {
                    if (source.bordersGS.ElementAt(i / 2) == null || source.bordersGS.ElementAt(i / 2).Count == 0)
                        return false;
                    else
                        return true;
                }
            }
            return false;
        }
        #endregion

        #region calculation logic
        //<summary>
        //method which controls whole navigation and returns route made of two "map layers" - worldroutes (list of clusters which will be visited on the route) 
            //and cluster routes (list of lists of byte[2], every inner list corresponds to cluster from world routes and contains inner points to go through
            //here will be optimalization in navigation, as the most used routes will be saved and there won't be need to recalculate them every time and world routes 
            //aren't so complex to count
        //</summary>
        
        public static Route totalRoute(WorldCluster source, WorldCluster destination, int sourceX, int sourceY, int destinationX, int destinationY, List<WorldCluster> route)
        {
            if (route == null)
            {
                expanded = new List<WorldCluster>(); //Expanded is initialized here, as countWorldRoute is recursive (iteratively calls itself until it gains result) and each initialization needs this list
                route = countWorldRoute(source, destination); //but as I'm looking in this code, I'm thinking if it isn't uselles, as I maybe could use the "route" list. Have to think it out...
                expanded = null;
            }
            List<List<byte[]>> nodes = new List<List<byte[]>>();
            //I'm counting routes for each cluster - that is what borders arrays are used for
            //later on I want to count them only once awhile - if the bordes change (unit will cut tree/build wall etc), but those routes could be staticly saved and not counted each time unit moves
            for (int i = 0; i < route.Count - 1; i++)
            {
                byte tmpDirection = directionTo(route.ElementAt(i), route.ElementAt(i + 1));
                try
                {
                    nodes.Add(routeThroughCluster(route.ElementAt(i), sourceX, sourceY, tmpDirection));
                }
                catch (Exception e)
                {
                    if (e.Message == "Corner full" || e.Message == "Route calculation exception")
                    {
                        return changeWorldRoute(route, i, sourceX, sourceY, destinationX, destinationY, tmpDirection); //Error while crossing from cluster i into cluster i+1
                    }
                }
                //at the end of the cluster, I need to decide3 where I am to set up correctly position in new cluster
                //this is not working correctly (you probabbly see it, but to be sure: if (X == Y == 9) -> jump to invalid location
                sourceX = (int)nodes.ElementAt(i).Last()[0]; //Error: Index was out of range. Must be non-negative and less than the size of the collection.
                sourceY = (int)nodes.ElementAt(i).Last()[1];
                if (sourceX == 0)
                    sourceX = 9;
                else
                    if (sourceX == 9)
                        sourceX = 0;
                if (sourceY == 0)
                    sourceY = 9;
                else
                    if (sourceY == 9)
                        sourceY = 0;
            }
            try
            {
                //counting not for transitions between clusters, but from last border to last point
                nodes.Add(routeThroughCluster(route.Last(), sourceX, sourceY, destinationX, destinationY));
            }
            catch (Exception e)
            {
                //this is pretty much tricky thing -> we can have point which is accesible (not occupied), but between two points in cluster there's no direct way, so we need to go through another cluster
                if (e.Message == "Route not available")  
                {
                    throw new Exception();
                }
            }

            Route r = new Route(); //object which is passed to MovingObject instance to follow (below in Move method)
            r.clusterRoutes = nodes;
            r.worldRoutes = route;
            return r;
        }
        //<summary>
        //method which changes navigation between clusters e.g.: when transition between clusters 1,1/0,0 is impossible -> go 1,1/1,0/0,0
        //</summary>
        private static Route changeWorldRoute(List<WorldCluster> route, int i, int sourceX, int sourceY, int destinationX, int destinationY, byte orientation)
        {
            //ToDo - make changes in List<WorldCluster> route
            return totalRoute(route.First(), route.Last(), sourceX, sourceY, destinationX, destinationY, route);
        }

        //<summary>
        //Counts elements of route in one cluster, in last one - using classical A* navigation
        //</summary>
        private static List<byte[]> routeThroughCluster(WorldCluster worldCluster, int sourceX, int sourceY, int destinationX, int destinationY)
        {
            List<byte[]> expanded = new List<byte[]>();
            byte[] expanding = new byte[2] { (byte)sourceX, (byte)sourceY };
            //Right now I'm not really sure what this means :D
            if (destinationX < 0)
                destinationX = 10 + destinationX;
            if (destinationY < 0)
                destinationY = 10 + destinationY;
            byte[] target = new byte[2] { (byte)destinationX, (byte)destinationY };
            byte[] toExpand = null;
            while (true)
            {
                if (expanding[0] == target[0] && expanding[1] == target[1])
                {
                    expanded.Add(target);
                    break;
                }
                double heuristics = double.MaxValue;
                //after those two nested loops I'll get best point to expand in next step
                //for: expandind.X - 1, expanding.X, expanding.X +1 (but if < 0 -> 0, if > 9 -> 9 to avoid IndexOutOfBounds)
                for (int x = (((expanding[0] - 1) < 0) ? 0 : expanding[0] - 1); x < ((expanding[0] + 2) > 9 ? 10 : expanding[0] + 2); x++)
                {//the same for Y
                    for (int y = (((expanding[1] - 1) < 0) ? 0 : expanding[1] - 1); y < ((expanding[1] + 2) > 9 ? 10 : expanding[1] + 2); y++)
                    {
                        var tmp = routeLength(x, y, target[0], target[1]); //return distance between 2 point (triangle)
                        if (tmp < heuristics && !worldCluster.unmovableContentGS[x, y].occupied) //if distance is lower and point is empty (maybe reverse approuch will be faster - ask if point is empty before heuristics)
                        {
                            bool contains = false;
                            foreach (byte[] b in expanded) //searching, if I visited x/y
                            {
                                if (b[0] == x && b[1] == y)
                                {
                                    contains = !contains;
                                    break;
                                }
                            }
                            if(!contains) //if I haven't visited the point
                            {
                                heuristics = tmp; //I setup new heuristics
                                toExpand = new byte[2] { (byte)x, (byte)y }; //and change point to expand
                            }
                        }
                    }
                }
                if (expanded.Count < 25) //control if I ain't stuck somewhere, cluster has 100 fields/nodes, so I assume, that if there's way to be found by A*, it should be shorter than 25 nodes -> to be discussed/changed
                {
                    expanded.Add(expanding);
                    expanding = toExpand;
                }
                else
                {
                    try
                    {
                        return alternativeRouteCalculation(worldCluster, sourceX, sourceY, destinationX, destinationY);
                    }
                    catch (Exception e)
                    {
                        if (e.Message == "Error while calculating in cluster route! Even different logic approach didn't help!")
                        {
                            return multiclusterRoute(worldCluster, sourceX, sourceY, destinationX, destinationY);
                        }
                    }
                }
            }
            return expanded;
        }

        //<summary>
        //Counts route through one cluster - from border to border
        //</summary>
        private static List<byte[]> routeThroughCluster(WorldCluster worldCluster, int sourceX, int sourceY, byte orientation)
        {
            byte[] target = new byte[2] { 255, 255 };

            #region Calculating coordinates of border crossing to use
            if (orientation % 2 == 0) //deciding if I'm going through border or corner
            {
                byte border = (byte)(orientation / 2);
                double length = Double.MaxValue;
                var targetBorder = worldCluster.bordersGS[border];
                double heuristics = double.MaxValue;
                foreach (int[] borderCoordinate in targetBorder)
                {
                    heuristics = routeLength(sourceX, sourceY, borderCoordinate[0], borderCoordinate[1]);
                    if (heuristics == 0)
                        break; //if heuristics == 0, we're at target
                    if (heuristics < length)
                    {
                        target = new byte[] { (byte)borderCoordinate[0], (byte)borderCoordinate[1] };
                        length = heuristics;
                    }
                }
            }
            #endregion

            #region Asking if I can go through corner
            else
            {
                switch (orientation)    //tenhle switch potrebuje "oelsovat" -> if(!isCornerEmpty) -> pridas jeden ze (sude) sousednich clusteru a on nepujde napric, ale "okolo".
                //Priklad: jdu z clusteru 1/1 do clusteru 0/0; roh je plnej -> zmen trasu z {1/1,0/0} na {1/1, 1/0, 0/0}
                //target je jednoduchej -> pujdu pres tu nejblizsi hranici rohu
                                        //this switch needs to expand, bcs if (nested if == false) -> target == (255,255) -> falls to exception
                                        //else {getRouteAround()} -> insert into route Lists new entry with neighbor cluster...
                {
                    case 1:
                        {
                            if (isCornerEmpty(worldCluster, Program.world.clusterList.Where(w => w.X == worldCluster.X + 1 && w.Y == worldCluster.Y - 1).FirstOrDefault()))
                                target = new byte[2] { 9, 0 };
                            else
                                throw new Exception ("Corner full");
                            break;
                        }

                    case 3:
                        {
                            if (isCornerEmpty(worldCluster, Program.world.clusterList.Where(w => w.X == worldCluster.X + 1 && w.Y == worldCluster.Y + 1).FirstOrDefault()))
                                target = new byte[2] { 9, 9 };
                            else
                                throw new Exception("Corner full");
                            break;
                        }

                    case 5:
                        {
                            if (isCornerEmpty(worldCluster, Program.world.clusterList.Where(w => w.X == worldCluster.X - 1 && w.Y == worldCluster.Y + 1).FirstOrDefault()))
                                target = new byte[2] { 0, 9 };
                            else
                                throw new Exception("Corner full");
                            break;
                        }

                    case 7:
                        {
                            if (isCornerEmpty(worldCluster, Program.world.clusterList.Where(w => w.X == worldCluster.X - 1 && w.Y == worldCluster.Y - 1).FirstOrDefault()))
                                target = new byte[2] { 0, 0 };
                            else
                                throw new Exception("Corner full");
                            break;
                        }
                }
            

            }
            #endregion

            if (target[0] > 10 || target[1] > 10)
                throw new Exception("Route calculation exception");

            List<byte[]> expanded = new List<byte[]>();
            List<byte[]> route = new List<byte[]>();
            byte[] expanding = new byte[2] { (byte)sourceX, (byte)sourceY };
            byte[] toExpand = null;
            route.Add(expanding);
            while (true)
            {
                if (expanding[0] == target[0] && expanding[1] == target[1])
                {
                    expanded.Add(target);
                    break;
                }
                double heuristics = double.MaxValue;
                for (int i = (((expanding[0] - 1) < 0) ? 0 : expanding[0] - 1); i < ((expanding[0] + 2) > 9 ? 10 : expanding[0] + 2); i++)
                {
                    for (int j = (((expanding[1] - 1) < 0) ? 0 : expanding[1] - 1); j < ((expanding[1] + 2) > 9 ? 10 : expanding[1] + 2); j++)
                    {
                        if (i != expanding[0] || j != expanding[1])
                        {
                            var tmp = routeLength(i, j, target[0], target[1]);
                            if (tmp < heuristics && !worldCluster.unmovableContentGS[i, j].occupied)
                            {
                                heuristics = tmp;
                                toExpand = new byte[2] { (byte)i, (byte)j };
                            }
                        }
                    }
                }
                expanded.Add(expanding);
                expanding = toExpand;
            }
            return expanded;
        }

        //<summary>
        //CZ: jakmile nelze nalezt cestu uvnitr clusteru, musi se jit k cili pres jiny cluster
        //CZ: tohle bude teda prdel :D
        //EN: If I was unable to find route through cluster, I need to go around
        //EN: this is gonna be pretty much fucked up :D
        //</summary>
        private static List<byte[]> multiclusterRoute(WorldCluster worldCluster, int sourceX, int sourceY, int destinationX, int destinationY)
        {
            throw new NotImplementedException("This is one fucked up method to be done");
        }
        //<summary>
        //CZ: metoda pouzivajici neinformovane (pomalejsi) prohledavani stavoveho prostoru v momente, kdy A* algorytmus neni schopen najit cestu
        //CZ: pouziva algorytmus BFS - Prohledavani do sirky
        //EN: method using slower, not informed search of state space (is more immune to stuck in "no thoroughfare" parts of map and is more likely to be able to navigate around)
        //EN: using BFS algorythm (breadth-first search)
        //</summary>
        private static List<byte[]> alternativeRouteCalculation(WorldCluster worldCluster, int sourceX, int sourceY, int destinationX, int destinationY)
        {
            List<byte[]> open = new List<byte[]>();
            List<byte[]> closed = new List<byte[]>();
            List<byte[]> route = new List<byte[]>();

            open = expand(worldCluster, sourceX, sourceY); //returns list of free positions around
            closed.Add(new byte[]{ (byte)sourceX, (byte)sourceY});
            bool jump = false;      //condition whether target is found
            var tmp = expand(worldCluster, open.First());
            while (open.Count > 0)
            {
                foreach (byte[] b in tmp)
                {
                    if (b[0] == destinationX && b[1] == destinationY)
                    {
                        jump = !jump;
                        closed.Add(b);
                        break;
                    }
                    if (!(closed.Where(w => w[0] == b[0] && w[1] == b[1]).Count() > 0) && open.Where(w => w[0] == b[0] && w[1] == b[1]).Count() < 1)
                    {
                        open.Add(new byte[] { b[0], b[1] });
                    }
                }
                if (!jump)
                {
                    closed.Add(new byte[] { open.First()[0], open.First()[1] });
                    open.RemoveAt(0);
                    tmp = expand(worldCluster, open.First());
                }
                else
                    break;
            }

            if (jump)
            {
                while (route.Last()[0] != sourceX && route.Last()[1] != sourceY)
                {
                    tmp = expand(worldCluster, closed.Last());
                    route.Add(closed.Last());
                    int pointer = int.MaxValue;
                    foreach (byte[] b in tmp)
                    {
                        for (int i = closed.Count; i >= 0; i--)
                        {
                            if (closed.ElementAt(i)[0] == b[0] && closed.ElementAt(i)[1] == b[1])
                            {
                                if (i < pointer)
                                {
                                    pointer = i;
                                }
                            }
                        }
                    }
                    for (int j = pointer + 1; j < closed.Count; j++)
                    {
                        closed.RemoveAt(j);
                    }
                    if (route.Count > 50)
                        throw new Exception("Target not found in reasonable count of steps! (alternativeRouteCalculation)");
                }
                return route;
            }
            else
            {
                throw new Exception("Target not found! (alternativeRouteCalculation)");
            }

            
        }
        //<summary>
        //Methods for BFS - returns list of free nodes around input node
        //</summary>
        private static List<byte[]> expand(WorldCluster worldCluster, byte[] b)
        {
            return expand(worldCluster, b[0], b[1]);
        }

        private static List<byte[]> expand(WorldCluster worldCluster, int sourceX, int sourceY)
        {
            List<byte[]> result = new List<byte[]>();
            for (int i = ((sourceX - 1) < 0 ? 0 : sourceX - 1); i < ((sourceX + 2 > 9) ? 9 : sourceX + 2); i++)
            {
                for (int j = ((sourceY - 1) < 0 ? 0 : sourceY - 1); j < ((sourceY + 2 > 9) ? 9 : sourceY + 2); j++)
                {
                    if (!worldCluster.unmovableContentGS[i, j].occupied)
                        result.Add(new byte[] { (byte)i, (byte)j });
                }
            }
            if (result.Count < 1)
                throw new Exception("Nowhere to continue!"); //this exception won't occur, as expand doesn't have info about nodes already expanded, so it will return minimally parent node
            return result;
        }

        //private static List<byte[]> getTargetSite(byte p, WorldCluster src)
        //{
        //    List<byte[]> target = new List<byte[]>();
        //    switch (p)
        //    {
        //        case 8:
        //            {
        //                Console.WriteLine("Neco je spatne Jime: (MonementLogic.countClusterRoute)");
        //                return null;
        //            }
        //        case 7:
        //            {
        //                target.Add(new byte[2] { 0, 0 });
        //                break;
        //            }
        //        case 6:
        //            {
        //                target.Add(new byte[2] { 0, 9 });
        //                break;
        //            }
        //        case 5:
        //            {
        //                target.Add(new byte[2] { 9, 9 });
        //                break;
        //            }
        //        case 4:
        //            {
        //                target.Add(new byte[2] { 9, 0 });
        //                break;
        //            }
        //        case 3:
        //            {
        //                foreach (int i in src.bordersGS[3])
        //                {
        //                    target.Add(new byte[2] { 0, (byte)i });
        //                }
        //                break;
        //            }
        //        case 2:
        //            {
        //                foreach (int i in src.bordersGS[2])
        //                {
        //                    target.Add(new byte[2] { (byte)i, 9 });
        //                }
        //                break;
        //            }
        //        case 1:
        //            {
        //                foreach (int i in src.bordersGS[3])
        //                {
        //                    target.Add(new byte[2] { 9, (byte)i });
        //                }
        //                break;
        //            }
        //        case 0:
        //            {
        //                foreach (int i in src.bordersGS[3])
        //                {
        //                    target.Add(new byte[2] { (byte)i, 0 });
        //                }
        //                break;
        //            }
        //    }
        //    return target;
        //}

        private static byte directionTo(WorldCluster worldCluster1, WorldCluster worldCluster2)
        {
            if (worldCluster1.neigbourhood.Contains(worldCluster2))
            {
                for (int i = 0; i < worldCluster1.neigbourhood.Count(); i++)
                {
                    if (worldCluster1.neigbourhood[i] == worldCluster2)
                        return (byte)(i);
                }
            }
            throw new Exception("Not in neighborhood");
        }

        private static List<WorldCluster> expanded;

        //CZ: rekurzivni metoda, ktera projde okoli nodu, vyhodnoti ten nejlepsi k expanzi a pak zavola sama sebe a nahradi WorldCluster source nodem s nejnizsi heuristickou funkci
        //EN: recursive method, which searches neighborhood, evaluates the best cluster to expand next and calls itself
        public static List<WorldCluster> countWorldRoute(WorldCluster source, WorldCluster destination)
        {
            List<WorldCluster> route = new List<WorldCluster>();
            route.Add(source);
            expanded.Add(source);
            if (source == destination)
                return route;

            double heuristics = routeLength(source.X, source.Y,destination.X, destination.Y); //using the same method with nodes & clusters for evaluating heuristics 
            WorldCluster toExpand = null;
            for (int i = source.X-1; i < source.X+2; i++)
            {
                for (int j = source.Y-1; j < source.Y+2; j++)
                {
                    if (j != source.Y || i != source.X) //serching all around but source
                    {
                        WorldCluster tmp = Program.world.clusterList.Where(w => w.X == i && w.Y == j).FirstOrDefault();
                        if (tmp == destination && haveCoomonBorder(source, tmp))
                        {
                            route.Add(tmp);
                            return route;
                        }
                        if(!expanded.Contains(tmp) && tmp != null)
                        {
                            double tmpRouteLength = routeLength(tmp, destination);
                            if (tmpRouteLength < heuristics && tmpRouteLength != 0) //if(tmpRouteLength == 0) here -> tmp == destination, BUT haveCoomonBorder == false!
                            {
                                if (Math.Abs(source.X - tmp.X) != Math.Abs(source.Y - tmp.Y)) //ptam se na roh
                                {
                                    if (haveCoomonBorder(source, tmp))
                                    {
                                        heuristics = routeLength(tmp, destination);
                                        toExpand = tmp;
                                    }
                                }
                                else
                                {
                                    if (isCornerEmpty(source, tmp))
                                    {
                                        heuristics = routeLength(tmp, destination);
                                        toExpand = tmp;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            route.AddRange(countWorldRoute(toExpand, destination));
            return route;
        }
        #endregion

        #region math
        public static double routeLength(int sourceX, int sourceY, int destinationX, int destinationY)
        {
            return Math.Sqrt(Math.Pow(sourceX - destinationX, 2) + Math.Pow(sourceY - destinationY, 2));
        }

        public static double routeLength(Point a, Point b)
        {
            return (routeLength(a.X, a.Y, b.X, b.Y));
        }

        public static double routeLength(WorldCluster source, WorldCluster destination)
        {
            if (source == destination)
                return 0;
            else
                return Math.Pow(0.5, Math.Pow(source.X - destination.X, 2) + Math.Pow(source.Y - destination.Y, 2));
        }
        #endregion

        //<summary>
        //method called from Game1.Update
        //gets target cluster and decides what to do, if target is not searched territory
        //invokes calculation of route
        //starts new thread, which is movement of MovingObject
        //</summary
        internal static void Move(MovingObject movingObject, Point clusterPoint, Point endPoint, bool invokeAction) //this method is still valid - will be used if player navigates unit into unsearched part of state space
        {
            WorldCluster destination = Program.world.clusterList.Where(w => w.X == clusterPoint.X && w.Y == clusterPoint.Y).FirstOrDefault(); //pozice vzhledem ke clusteru (0,0) -> prepsat vzhledem k movingObject.inCluster!!!
            if (destination == null)
            {
                destination = Program.world.clusterList.Where(w => w.X == findNearestPoint(clusterPoint.X, clusterPoint.Y, typeof(WorldCluster)).X && w.Y == findNearestPoint(clusterPoint.X, clusterPoint.Y, typeof(WorldCluster)).Y).FirstOrDefault();
            }
            Route total = totalRoute(movingObject.inCluster, destination, movingObject.pos.X, movingObject.pos.Y, endPoint.X, endPoint.Y, null);

            Thread move = new Thread(() => movingObject.move(total)); //I don't know, where do I go -> I only move
            move.Start();
        }

        internal static void Move(MovingObject movingObject, WorldObject navigateTo, bool invokeAction) //use this method when I know WorldObject where I navigate -> I don't have to calculate coordinates from mouse
        {
            //if invokeAction : search neighborhood of destination point to get position where unit has to stand and begin action
            Route total;
            if (invokeAction)
            {
                Point inClusterDestination = getFinalPoint(movingObject, navigateTo);
                total = totalRoute(movingObject.inCluster, navigateTo.inCluster, movingObject.pos.X, movingObject.pos.Y, inClusterDestination.X, inClusterDestination.Y, null);
            }
            else
                total = totalRoute(movingObject.inCluster, navigateTo.inCluster, movingObject.pos.X, movingObject.pos.Y, navigateTo.position.X, navigateTo.position.Y, null);
            Thread move = new Thread(() => movingObject.move(total, navigateTo)); //I know where do I go -> I may want to do sth there
            move.Start();
        }

        private static Point getFinalPoint(MovingObject movingObject, WorldObject navigateTo)
        {
            List<WorldObject> objectsAround = new List<WorldObject>();
            Point source = new Point();
            if (movingObject.inCluster == navigateTo.inCluster) //getting source point - if target is in cluster as moving object, it is clear
            {
                source = movingObject.pos;
            }
            else //but if searching between multiple clusters, I try to find ideal solution from diroction from which the unit will come
            {
                Point direction = new Point(navigateTo.inCluster.X - movingObject.inCluster.X, navigateTo.inCluster.Y - movingObject.inCluster.Y);
                if (direction.Y != 0)
                {
                    if (Math.Abs(direction.X) / Math.Abs(direction.Y) < 1)
                    {
                        if (direction.Y < 0)
                            source = new Point(navigateTo.inCluster.bordersGS[2].First()[0], navigateTo.inCluster.bordersGS[0].First()[1]);
                        else
                            source = new Point(navigateTo.inCluster.bordersGS[0].First()[0], navigateTo.inCluster.bordersGS[0].First()[1]);
                    }
                    else
                    {
                        if(direction.X < 0)
                            source = new Point(navigateTo.inCluster.bordersGS[1].First()[0], navigateTo.inCluster.bordersGS[0].First()[1]);
                        else
                            source = new Point(navigateTo.inCluster.bordersGS[3].First()[0], navigateTo.inCluster.bordersGS[0].First()[1]);
                    }
                }
            }

            int around = 1;
            while (true)
            {
                for (int x = (navigateTo.position.X - around < 0 ? 0 : navigateTo.position.X - around); x < (navigateTo.position.X + around + 1 > 9 ? 9 : navigateTo.position.X + around + 1); x++)
                {
                    for (int y = (navigateTo.position.Y - around < 0 ? 0 : navigateTo.position.Y - around); y < (navigateTo.position.Y + around + 1 > 9 ? 9 : navigateTo.position.Y + around + 1); y++)
                    {
                        if (!navigateTo.inCluster.unmovableContentGS[x, y].occupied && (x != 0 || y != 0))
                        {
                            objectsAround.Add(navigateTo.inCluster.unmovableContentGS[x, y]);
                        }
                    }
                }
                if (objectsAround.Count() == 0 || objectsAround == null) //if I haven't found any free space next to one clicked by user, I'll increase search space and loop again
                {
                    around++;
                }
                else
                    break;
            }
            double heuristics = double.MaxValue;
            Point returnPoint = new Point();
            foreach (WorldObject wo in objectsAround)
            {
                if(heuristics > routeLength(source.X, source.Y, wo.position.X, wo.position.Y))
                {
                    heuristics = routeLength(source.X, source.Y, wo.position.X, wo.position.Y);
                    returnPoint = new Point(wo.position.X, wo.position.Y); //choosing the closest point which is not occupied
                }
            }
            return returnPoint;
        }

        private static Point findNearestPoint(int p, int p_2, Type t)
        {
            double heuristics = double.MaxValue;
            Point result = new Point();
            if (t.GetType() == typeof(WorldCluster))
            {
                foreach (WorldCluster wc in Program.world.clusterList)
                {
                    var tmp = routeLength(wc.X, wc.Y, p, p_2);
                    if (tmp < heuristics)
                    {
                        heuristics = tmp;
                        result = new Point(wc.X, wc.Y);
                    }
                }
            }
            else
            {

            }
            return result;
        }


    }
    public class Route
    {
        public List<List<byte[]>> clusterRoutes;
        public List<WorldCluster> worldRoutes;
    }
}
